/*
 * File:   c-stepper.c
 * Author: David Mézière <dmeziere@gmail.com>
 *
 * Created on 17 juillet 2011, 21:42
 *
 * use "gcc -lk8055 -o c-stepper c-stepper.c" for compiling.
 * It will output some warnings, nothing to fear of.
 */

#include <stdio.h>
#include <stdlib.h>
#include <k8055.h>
#include <sys/time.h>

#define false 0
#define true 1

// Tableau des séquences. Les dimensions sont le mode, le moteur, et le pas.
// Certains modes ont 8 pas, d'autres 4. Ceux de 4 pas ont étés doublés afin
// de répondre au besoin du langage d'avoir des tableaux uniformes. Les 3
// dimensions sont mode, moteur et pas.
int aSequences[3][5][8] = {

    // Mode single step, activation d'une seule phase par pas.
    // Plus faible consommation, plus faible couple.
    {
        {0, 0, 0, 0, 0, 0, 0, 0},               // Inutilisé, présent par compatibilité.
        {1, 2, 4, 8, 1, 2, 4, 8},               // Séquence du moteur 1.
        {16, 32, 64, 128, 16, 32, 64, 128},     // Séquence du moteur 2.
        {17, 34, 68, 136, 17, 34, 68, 136},     // Moteur 1 et 2 dans le même sens.
        {129, 66, 36, 24, 129, 66, 36, 24}      // Moteur 1 et 2 en sens inverse.
    },

    // Mode full step, activation des phases par paire.
    // Consommation doublée par rapport au single step, plus fort couple.
    {
        {0, 0, 0, 0, 0, 0, 0, 0},               // Inutilisé, présent par compatibilité.
        {3, 6, 12, 9, 3, 6, 12, 9},             // Séquence du moteur 1.
        {48, 96, 192, 144, 48, 96, 192, 144},   // Séquence du moteur 2.
        {51, 102, 204, 153, 51, 102, 204, 153}, // Moteur 1 et 2 dans le même sens.
        {195, 102, 60, 153, 195, 102, 60, 153}  // Moteur 1 et 2 en sens inverse.
    },

    // Mode half step, activation des phases une par une, puis par paire.
    // Consommation doublée par rapport au single step, plus fort couple.
    // Précision doublée par activation des demi-pas.
    // Vitesse divisée par deux, de par le doublement des pas.
    {
        {0, 0, 0, 0, 0, 0, 0, 0},               // Inutilisé, présent par compatibilité.
        {1, 3, 2, 6, 4, 12, 8, 9},              // Séquence du moteur 1.
        {16, 48, 32, 96, 64, 192, 128, 144},    // Séquence du moteur 2.
        {17, 51, 34, 102, 68, 204, 136, 153},   // Moteur 1 et 2 dans le même sens.
        {129, 195, 66, 102, 36, 60, 24, 153}    // Moteur 1 et 2 en sens inverse.
    }
};

// Sequence length.
int iSeqLength = 8;

// Last step used on each motor.
int iLastPhase[5] = {0, 0, 0, 0, 0};

// Last sense used on each motor.
int iLastSense[5] = {0, 0, 0, 0, 0};

char* sModeNames[3] = {"Single step", "Full step", "Half step"};

// Default parameters.
int iVerbose = false;
int iPort = 0;
int iMode = 0;

unsigned long int start, lastCall = 0;

inline unsigned long int time_msec ( void ) {
	struct timeval t;
        struct timezone tz;
	gettimeofday (&t, &tz);
	return (1000 * t.tv_sec) + (t.tv_usec / 1000);
}

/*
 * Main loop. Scans the command-line params and do the corresponding action.
 *
 * @param int argc Argument count
 * @param int params Arguments array
 *
 * @return int 0 if ok, 1 if ko.
 */
int main(int argc, char** params) {

    int i = 0;
    int count = 0;

    start = time_msec();

    // The switches that don't need to open the board are checked first.
    for (i = 1; i < argc; i++) {

        // Usage informations
        if (!strcmp(params[i], "-h") || !strcmp(params[i], "--help")) {
            displayHelp(params);
            return(-1);

        // Verbose mode
        } else if (!strcmp(params[i], "-v") || !strcmp(params[i], "--verbose")) {
            lastCall = time_msec();
            printf("[%05d] Entering verbose mode.\n", (int)(lastCall - start));
            iVerbose = true;

        // Port number
        } else if (!strncmp(params[i], "-p:", 3)) {
            if(!setPort(params[i] + 3)) {
                return(1);
            }
        } else if (!strncmp(params[i], "--port:", 7)) {
            if(!setPort(params[i] + 7)) {
                return(1);
            }

        // Stepping mode
        } else if (!strncmp(params[i], "-m:", 3)) {
            if(!setMode(params[i] + 3)) {
                return(1);
            }
        } else if (!strncmp(params[i], "--mode:", 7)) {
            if(!setMode(params[i] + 7)) {
                return(1);
            }

        }
    }

    // Opening of the board
    if (OpenDevice(iPort) < 0 ) {

        printf("Unable to open the k8055 board (port: %d).\n", iPort);
        return false;

    } else {

        if(iVerbose) {
            lastCall = time_msec();
            printf("[%05d] Opening the k8055 board (port: %d).\n", (int)(lastCall - start), iPort);
        }

    }

    // The switches that need to open the board are then checked.
    for (i = 1; i < argc; i++) {

        // Usage informations
        if (!strcmp(params[i], "-h") || !strcmp(params[i], "--help")) {
            // This switch has already been checked.
            // But we must not throw an error.

        // Verbose mode
        } else if (!strcmp(params[i], "-v") || !strcmp(params[i], "--verbose")) {
            // This switch has already been checked.
            // But we must not throw an error.

        // Port number
        } else if (!strncmp(params[i], "-p:", 3) || !strncmp(params[i], "--port:", 7)) {
            // This switch has already been checked.
            // But we must not throw an error.

        // Stepping mode
        } else if (!strncmp(params[i], "-m:", 3) || !strncmp(params[i], "--mode:", 7)) {
            // This switch has already been checked.
            // But we must not throw an error.

        // Motor 1 steps.
        } else if (!strncmp(params[i], "-1:", 3)) {
            if(!sscanf(params[i] + 3, "%d", &count)) {
                printf("Incorrect step count: %s.\n", params[i] + 3);
                return(1);
            }
            setSteps(1, count);
        } else if (!strncmp(params[i], "--motor1:", 9)) {
            if(!sscanf(params[i] + 9, "%d", &count)) {
                printf("Incorrect step count: %s.\n", params[i] + 9);
                return(1);
            }
            setSteps(1, count);

        // Motor 2 steps.
        } else if (!strncmp(params[i], "-2:", 3)) {
            sscanf(params[i] + 3, "%d", &count);
            setSteps(2, count);
        } else if (!strncmp(params[i], "--motor2:", 9)) {
            sscanf(params[i] + 9, "%d", &count);
            setSteps(2, count);

        // Both motors steps.
        } else if (!strncmp(params[i], "-b:", 3)) {
            sscanf(params[i] + 3, "%d", &count);
            setSteps(3, count);
        } else if(!strncmp(params[i], "--both:", 7)){
            sscanf(params[i] + 7, "%d", &count);
            setSteps(3, count);

        // Both motors steps, reversed.
        } else if (!strncmp(params[i], "-r:", 3)) {
            sscanf(params[i] + 3, "%d", &count);
            setSteps(4, count);
        } else if (!strncmp(params[i], "--reversed:", 11)){
            sscanf(params[i] + 11, "%d", &count);
            setSteps(4, count);

        } else {
            printf("Unknown option.\n");
            return (1);
        }
    }

    // Putting down all outputs.
    WriteAllDigital((long)0);

    CloseDevice();

    if(iVerbose == true) {
        lastCall = time_msec();
        printf("[%05d] Closing the k8055 board (port: %d).\n", (int)(lastCall - start), iPort);
    }

    return (EXIT_SUCCESS);
}

/*
 * Write help to standard output.
 *
 * @param char *params Parameters array.
 *
 * @return int 1 if ok (always).
 */
int displayHelp (char* params[] ) {

    printf("%s \n", basename(params[0]));
    printf("   -h, --help: Displays these informations.\n");
    printf("   -v, --verbose: Display the orders as they are executed.\n");
    printf("   -p, --port: K8055 board address.\n");
    printf("      0: SK5 and SK6 are closed on the board (Default).\n");
    printf("      1: SK5 is open and SK6 is closed on the board.\n");
    printf("      2: SK5 is closed and SK6 is open on the board.\n");
    printf("      3: SK5 and SK6 are open on the board.\n");
    printf("   -m, --mode: Motor phases activation mode.\n");
    printf("      0: Single step mode. Activation of one phase at once. Weaker consumption and couple (Par défaut).\n");
    printf("      1: Full step mode. Activation of phases per pair. Consumption and couple doubled.\n");
    printf("      2: Half step mode. Mix of the two preceedings. Consumption, couple and precision doubled.\n");
    printf("   -1, --motor1: Number of steps for motor 1.\n");
    printf("   -2, --motor2: Number of steps for motor 2.\n");
    printf("   -b, --both: Number of steps for motor 1, motor 2 will follow the same way.\n");
    printf("   -r, --reversed: Number of steps for motor 1, motor 2 will follow the opposite way.\n");
    printf("\n");
    printf("A step count can be positive or negative. In reversed mode, you drive motor 1, motor 2 will be reversed.\n");
    printf("\n");
    printf("The -h, -v, -p and -m switches must be used first and once. But the others can be duplicated.\n");
    printf("You cannot drive multiple boards with a single command. But a script can send multiple command lines.\n");
    printf("Example: %s --motor1:+50 --motor2:-50 --motor1:-50 --motor2:+50 --both:+100 --both:-100\n", basename(params[0]));

    return true;
}

/*
 * Sets the board port.
 *
 * @param char* value String value to be converted as int.
 *
 * @return int 1 if ok, 0 if ko.
 */
int setPort (char* value) {

    int tempPort = 0;

    // The port must be an integer.
    if(!sscanf(value, "%d", &tempPort)) {
        printf("Incorrect port: %s.\n", value);
        return false;
    }

    // The port must be between 0 and 3 included.
    if(tempPort < 0 || tempPort > 3) {
        printf("Incorrect port: %d.\n", tempPort);
        return false;
    }

    // If everything's ok the new value is applyed.
    iPort = tempPort;

    // If in verbose mode, the new mode is displayed.
    if(iVerbose) {
        lastCall = time_msec();
        printf("[%05d] Switching to port %d.\n", (int)(lastCall - start), iPort);
    }

    return true;
}

/*
 * Sets the stepping mode.
 *
 * @param char* value String value to be converted as int.
 *
 * @return int 1 if ok, 0 if ko.
 */
int setMode (char* value) {

    int tempMode = 0;

    // The mode must be an integer.
    if(!sscanf(value, "%d", &tempMode)) {
        printf("Incorrect mode: %s.\n", value);
        return false;
    }

    // The mode must be between 0 and 2 included.
    if(tempMode < 0 || tempMode > 2) {
        printf("Incorrect mode: %d.\n", tempMode);
        return false;
    }

    // If everything's ok the new value is applyed.
    iMode = tempMode;

    // If in verbose mode, the new mode is displayed.
    if(iVerbose) {
        lastCall = time_msec();
        printf("[%05d] Switching to %s mode.\n", (int)(lastCall - start), sModeNames[iMode]);
    }

    return true;
}

/*
 * Sends the steps sequences to the card.
 *
 * @param int iMotor Motor index
 * @param int iQuantity Quantity of steps to send
 *
 * @return int 1 if ok, 0 if ko.
 */
int setSteps (int iMotor, int iQuantity) {

    // iMotor must be between 1 and 4 included.
    if(iMotor < 1 || iMotor > 4) {
        printf("Invalid motor number.\n");
        return false;
    }

    // Output of the order.
    if (iVerbose) {
        lastCall = time_msec();

        if(iMotor == 1 || iMotor == 2) {
            printf("[%05d] Motor %d: %+d steps.\n", (int)(lastCall - start), iMotor, iQuantity);
        } else if (iMotor == 3) {
            printf("[%05d] Motor 1: %+d steps. Motor 2: %+d steps.\n", (int)(lastCall - start), iQuantity, iQuantity);
        } else if (iMotor == 4) {
            printf("[%05d] Motor 1: %+d steps. Motor 2: %+d steps.\n", (int)(lastCall - start), iQuantity, -iQuantity);
        }
    }

    int iStep = 0;
    int iStepPhase = iLastPhase[iMotor];

    if(iLastSense[iMotor] < 0 && iQuantity >= 0) {
        iStepPhase++;
    } else if(iLastSense[iMotor] == 0 && iQuantity < 0) {
        iStepPhase--;
    }

    while (iStep < abs(iQuantity)) {

        if(iQuantity < 0) {

            iStepPhase--;

            if(iStepPhase < 0) {
                iStepPhase = iSeqLength + iStepPhase;
            }

        } else {

            if(iStepPhase == iSeqLength) {
                iStepPhase = 0;
            }

        }

        // Envoi de la valeur calculée à la carte k8055.
        WriteAllDigital((long)aSequences[iMode][iMotor][iStepPhase]);

        // Incrementing or decrementing the steps, as asked.
        if(iQuantity >= 0) {

            iStepPhase++;

            if(iStepPhase == iSeqLength) {
                iStepPhase = 0;
            }

        } else {

            if(iStepPhase < 0) {
                iStepPhase = iSeqLength - 1;
            }

        }

        iStep++;
    }

    // Reminding the laste rotaton sense.
    if (iQuantity >= 0) {
        iLastSense[iMotor] = 0;
    } else {
        iLastSense[iMotor] = - 1;
    }

    // Reminding the state of the motor for next use.
    iLastPhase[iMotor] = iStepPhase;

    return true;

}
